#ifndef F_CPU
#define F_CPU 8000000UL
#endif

#include "counter.h"

#include <avr/io.h>
#include <avr/interrupt.h>

volatile uint8_t T1_HCOUNTER;
volatile uint8_t mov;

ISR(CNT_VEC) {
  // Wheel sensor got a pulse

  static uint8_t tries;

  // If the pulse was too fast, ignore it.
  if ((T1_HCOUNTER==0) & (TCNT1<CNT_TIMEMIN)) return;
  // If not, first check if movement is starting
  else {
    // If it is, see if it confirms
    if (mov==0) {
      tries++;
      if (tries>TRIES_FOR_MOV) {
	mov=1;
	cnt_sum = tries + cnt_sum_bkp;
	tries = 0;
      }
    }
    // If not, continue summing up
    else { cnt_sum++; }
    // Reset timer variables
    T1_HCOUNTER = 0;
    TCNT1 = 0;
  }

}

ISR(TIMER1_OVF_vect) {

  // Increment the High Register on TCNT1 overflow
  T1_HCOUNTER++;
  /* If it times out, or the wheel is too slow or
     the plane has stopped. Either way set mov=0 
     and reset the counter */
  if (T1_HCOUNTER>CNT_TIMEOUT) {
    cnt_sum_bkp = cnt_sum;
    counter_reset();
  }

}

void set_cnt_int(void){
  
  /* Enable external interrupt request */
  CNT_MSK |= _BV(CNT_PIN);
  /* Enable Timer Overflow Interrupts */
  TIMSK1 |= _BV(TOIE1);

}

void clr_cnt_int(void){
  
  /* Disable external interrupt request */
  CNT_MSK &= ~_BV(CNT_PIN);
  /* Disable Timer Overflow Interrupts */
  TIMSK1 &= ~_BV(TOIE1);

}

void counter_init(void) {

  /*** Timer 1 Settings ***/
  /* 8 prescaler for 2MHz clock */
  TCCR1B |= _BV(CS11);

  /*** External INT0 interrupt ***/
  /* Set INT0 pin to input with pull-up enabled*/
  DDRD &= ~(1<<PD2);
  PORTD |= (1<<PD2);
  /* Set INT0 to interrupt at falling edge */
  CNT_REG |= (1<<ISC01);
  
  /* Enable interrupts */
  set_cnt_int();
  /* Set counters to 0 */
  counter_reset();
  cnt_sum_bkp = 0;

}

void counter_reset(void) {

  T1_HCOUNTER = 0;
  TCNT1 = 0;
  cnt_sum = 0;
  to_dist = 0;
  land_dist = 0;
  mov = 0;

}

uint16_t counter_get_cnts(void) {

  uint16_t ret;
 
  if (cnt_sum) {
    ret = cnt_sum;
    cnt_sum = 0;
  }
  else ret = 0;

  return ret;

}

uint32_t counter_dist_of_cnts(uint16_t tot_count) {

  uint32_t ret;

  /* First convert from count to mm */
  ret = ((uint32_t) tot_count)*CNT_SUM2DIS;
  /* Then apply correction, if necessary */
  ret = ret*CNT_FACTOR_NUM/CNT_FACTOR_DEN;
  /* Finally convert to decifeet */
  ret = ret*MM_TO_dFT;
  
  return ret;

}
